#!/usr/bin/env python
#coding=utf-8
from os import curdir
from rospkg import RosPack

# Note: 只要机器人不断电，不停止通讯
# 在ROS节点一直运行的情况下编码器可视作绝对编码器

import sys
sys.path.append(RosPack().get_path('canopen_communication')+"/modular/")
import json
from modular_T85 import T85
from modular_I85 import I85
from modular_T100 import T100
from modular_I100 import I100
import math
import threading
from time import sleep
import rospy

class robot():


    def __init__(self):
        self.__robot_start_flag = False
        self.__robot_stop_flag = False
        self.__robot_state = {}
        self.__eds_file=RosPack().get_path('canopen_communication')+'/file/Copley.eds'
        self.__I1 = I100(1,self.__eds_file)
        self.__T2 = T100(2,self.__eds_file)
        self.__T3 = T100(3,self.__eds_file)
        self.__T4 = I85(4,self.__eds_file)
        self.__I5 = T85(5,self.__eds_file)
        self.__joints=[self.__I1,self.__T2,self.__T3,self.__T4,self.__I5]
        self.__robot_state_file = RosPack().get_path('birl_manipulator')+'/file/robot_state.json'
        self.__pos_joints=[]
        self.zero_pos=[]

    # 机器人启动接口
    def robot_start(self):
        try:
            for i in range(len(self.__joints)):
                self.__joints[i].start()
            self.__robot_start_flag=True
        except Exception as e:
            print("%s"%e)
            self.__robot_start_flag=False
            print("--Connect Robot Failed")
        pass

    # 机器人停止接口
    def robot_stop(self):
        for i in range(len(self.__joints)):
            self.__joints[i].stop()
            self.__joints[i].stop_communication()
    
    # 设置机器人为位置模式
    def set_position_mode(self):
        if self.__robot_start_flag:
            for i in range(len(self.__joints)):
                self.__joints[i].opmode_set('PROFILED POSITION')
            return True
        else:
            print("--robot haven't been connected--")
            return False

    # 发送关节位置,运动速度默认为3
    def send_position(self,pos,vel=5):
        """
            brief： send_position(pos,vel) //pos单位为deg,vel单位为deg/s
        """
        if self.__robot_start_flag:

            # 发送目标关节角度 
            for i in range(len(self.__joints)):
                self.__joints[i].sent_position(pos[i],vel)

        else:
            print("--robot haven't been connected--")
    
    # 获取机器人当前各个关节
    def get_joint_position(self):
        angles=[0,0,0,0,0]
        for i in range(len(self.__joints)):
            angles[i] = self.__joints[i].get_position()
        return angles
    

    # 更新关节信息
    # Cause the Note which in the head of this script
    # in birl_hardware_interface_v3.py this function is useless
    def __update_robot_state(self):
        """
            brief: __update_robot_state() // private func 用于读取机器人状态
        """
        try:
            with open(self.__robot_state_file,'r') as f:
                self.__robot_state=json.load(f)
                self.__pos_joints=self.__robot_state['joint_position']
        except Exception as e:
            print("%s"%e)
            self.__robot_state={}
            print("--Cann't load the robot_state")
    
    # 用于每次运动结束后记录机器人关节角(绝对坐标:-180~180)
    # Cause the Note which in the head of this script
    # in birl_hardware_interface_v3.py this function is useless
    def __record_robot_state(self):
        """
            brief:__record_robot_state() //private func 用于记录机器人状态
        """
        for i in range(len(self.__joints)):
            self.__pos_joints[i] = round(self.__joints[i].get_position()+self.__pos_joints[i],2)
        record_data = dict([('joint_position',self.__pos_joints)])
        with open(self.__robot_state_file,'w+') as f:
            json.dump(record_data,f)
        pass
    
 
        